#ifndef _FORWARD_DYNAMICS_H
#define _FORWARD_DYNAMICS_H

#include "Model.h"
#include "vl/VLd.h"
#include "TransformEvaluator.h"
#include <vector>
#include <iostream>
#include <fstream>
#include "Archive.h"
#include "mtxlib.h"

Mat3d ExtractRotationFromTransform(Mat4d& m);
Vec3d ExtractTranslationFromTransform(Mat4d& m);
double randDouble(double low, double high);

#define FILE_IO_OFF

enum TrackMode
{
	TM_Prescribe,
	TM_Planning
};

class ForwardDynamics
{
public:
	ForwardDynamics();
	~ForwardDynamics();
	void SetModel(Model* m);
	void SetTargetState(double* targetValue, double* targetVel);
	vector3 Update(double timeStep, double* indexedEnergy, TrackMode mode = TM_Prescribe);
	void CalculateMMatrix(Matd& mat);
	void CalculateCVector(Vecd& mat);
	Matd CalculateJacobian(BodyLink* b, const Vec3d& localPos);
	int GetDim();
	void CalculateDeriv(double* deriv);
	void GetState(double* currentState);
	void SetState(double* state);
	void SetStateAndVel(double* state, double* stateVel);
	void SetStateAndVelAsTarget();
	void SetExternalForce(const Vecd& force);
	Vecd CalculateGravityForce();
	const Vecd& GetInternalForce() const;
	Vecd ApproxdTdQdotdt();
	Vecd ApproxdTdQdot();
	Vecd ApproxdTdQ();
	Vec3d ApproxLinearMomentum();
	Vec3d ApproxAngularMomentum();
	double CalculateKineticEnergy();

	friend DecoArchive& operator<< (DecoArchive& Ar, const ForwardDynamics& obj);
	friend DecoArchive& operator>> (DecoArchive& Ar, ForwardDynamics& obj);

private:
#ifndef FILE_IO_OFF
	ofstream outFile;
	ofstream outMatlabFile;
#endif
	void calculateBodyM(int index, Matd& mat);
	void calculateBodyC(int index, Vecd& mat);
	void calculateBodyJW(int index, Matd& mat);
	void calculateBodyJWq(int index, Dof* q, Matd& mat);
	void calculateBodyJWDot(int index, Matd& mat);
	void calculateBodyJC(int index, Matd& mat);
	void calculateBodyJCq(int index, Dof* q, Matd& mat);
	void calculateBodyJCDot(int index, Matd& mat);

	void approxBodyJW(int index, Matd& mat);
	void approxBodyJWDot(int index, Matd& mat);
	void approxBodyJCDot(int index, Matd& mat);
	void approxBodyJC(int index, Matd& mat);
	double approxBodydTdQ(int index, int ithDof);
	double approxBodydTdQdot(int index, int ithDof);
	double approxBodydTdQdotdt(int index, int ithDof);
	double calculateBodyKineticEnergy(int index);
	Mat4d calculateTransformDot(BodyLink* b);
	Vecd calculateExternalForce();
	Vecd calculateForce();
	Vecd calculateInternalForce();
	void dumpDeviationFromPerfect();
	void dumpDerivatives(const Vecd& rhs, const Vecd& qDotDot, const Vecd& generalizedForce, const Vecd& qDot);
	void dumpMatlabFile();	
	void clear();

	Model* mModel;
	//Model* mTarget;
	Matd* mMMatrix;
	Vecd* mCVector;
	double* mTargetValueState;
	double* mTargetVelState;
	Vecd mExternalForce;
	Vecd mInternalForce;
	Vecd mQDotDot;

	vector<Matd*> mBodyJCMatrices;
	vector<Matd*> mBodyLastJCMatrices;
	vector<Matd*> mBodyJCDotMatrices;
	vector<Matd*> mBodyJWMatrices;
	vector<Matd*> mBodyLastJWMatrices;
	vector<Matd*> mBodyJWDotMatrices;
	vector<Matd*> mBodyMMatrices;
	vector<Vecd*> mBodyCVectors;
	vector<Vecd*> mBodyLastdTdQdot;

	double mTimeStep;
	int mFrameCount;

	double ks;
	double kd;
	
	TrackMode mMode;
};

#endif